package edu.arizona.sista.learn.model.motion;

import java.util.ArrayList;
import java.util.List;

import edu.arizona.sista.learn.model.Interval;
import edu.arizona.sista.learn.model.displaying.Event;
import edu.arizona.sista.learn.model.holding.Person;
import edu.arizona.sista.learn.model.holding.Scene;
import edu.arizona.sista.learn.util.Computable;

public class RectilinearTrajectory extends Event{
	public static int MIN_SIZE = 10;
	public static int MIN_LENGTH = 3;
	public Person _doer;
	public List<Track> _tracks;  
	
	public RectilinearTrajectory(Person p, List<Track> tracks, Interval time) {
		super(time);
		_doer = p;
		_tracks = tracks;
	}

	public static void getRectilinearTrajectories(Scene scene){
		for(Person p: scene._people){
			List<Track> tracks = p._tracks; 
			double R2Min = .95;
			int start = 0;
			int end = 1;
			double R2 = 1;
			List<PathWeight> list = new ArrayList<PathWeight>();

			while(end < tracks.size()){
				List<Track> path = tracks.subList(start, end + 1);
				PathWeight pw = Computable.linearRegression(path);

				//test that the direction of the last two vectors is the same

				if (pw.size() > 2){
					double[] v1 = pw.getSecondToLastVector();
					double[] v2 = pw.getLastVector();

					double a = v1[0] * v2 [0] + v1[1] * v2[1];
					if (a < 0){
						//remove last point: end of the path
						pw.removeLastPoint();
						list.add(pw);
						start = end - 1;
						continue;
					}
				}


				R2 = pw._weight;
				end++;
				if (end == tracks.size()) list.add(pw);
				else if (R2 <= R2Min){
					//remove last point: end of the path
					pw.removeLastPoint();
					list.add(pw);
					start = end - 1;
					continue;
				}
			}
			list = filter(list);

			for(PathWeight pw: list){
				scene.addEvent(new RectilinearTrajectory(p, pw._path, new Interval(pw._path.get(0).frame, pw._path.get(pw._path.size() - 1).frame)));
			}
		}
	}

	private static List<PathWeight> filter(List<PathWeight> list) {
		int i = 0;
		while(i < list.size()){
			PathWeight pw = list.get(i);
			if (pw.size() < MIN_SIZE) list.remove(i);
			else if (length(pw._path) < MIN_LENGTH) list.remove(i);
			else i++;
		}
		return list;
	}

	private static double length(List<Track> path) {
		Track t0 = path.get(0);
		Track t1 = path.get(path.size() - 1);
		double[] v = new double[]{t1.pos[0] - t0.pos[0], t1.pos[1] - t0.pos[1]};
		double norm = v[0] * v[0] + v[1] * v[1];
		return norm;
	}

	@Override
	public String toString() {
		String s1 = "isa(" + getVariableName() + ", RectilinearTrajectory)\n";
		String s2 = "perfomedBy(" + getVariableName() + ", " + _doer.getVariableName() + ")\n";
		String t = "t_"  + _time.start() + "-" + _time.end();
		String s3 = "temporalExtent(" + getVariableName() + ", " + t + ")\n";
		return s1 + s2 + s3;
	}
	
}
